#include "attitude_controller.h"

ErrorCode attitude_controller_init(AttitudeController* controller) {
    if (!controller) return ERROR_INVALID_INPUT;
    
    controller->current_attitude = quaternion_identity();
    controller->target_attitude = quaternion_identity();
    controller->current_angular_velocity = vector3d_zero();
    controller->target_angular_velocity = vector3d_zero();
    
    // 初始化PID控制器
    pid_init(&controller->pid_roll, PID_KP, PID_KI, PID_KD);
    pid_init(&controller->pid_pitch, PID_KP, PID_KI, PID_KD);
    pid_init(&controller->pid_yaw, PID_KP, PID_KI, PID_KD);
    
    controller->mode = ATTITUDE_MODE_IDLE;
    controller->control_torque[0] = 0.0;
    controller->control_torque[1] = 0.0;
    controller->control_torque[2] = 0.0;
    controller->control_frequency = CONTROL_FREQUENCY;
    controller->is_active = false;
    
    return SUCCESS;
}

ErrorCode pid_init(PID_Controller* pid, double kp, double ki, double kd) {
    if (!pid) return ERROR_INVALID_INPUT;
    
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    
    for (int i = 0; i < 3; i++) {
        pid->integral[i] = 0.0;
        pid->prev_error[i] = 0.0;
    }
    
    pid->integral_limit = 1.0;
    pid->output_limit = MAX_TORQUE;
    
    return SUCCESS;
}

ErrorCode pid_update(PID_Controller* pid, double error, double dt, double* output) {
    if (!pid || !output || dt <= 0.0) return ERROR_INVALID_INPUT;
    
    // 积分项
    pid->integral[0] += error * dt;
    
    // 限制积分
    pid->integral[0] = CLAMP(pid->integral[0], -pid->integral_limit, pid->integral_limit);
    
    // 微分项
    double derivative = (error - pid->prev_error[0]) / dt;
    
    // PID输出
    *output = pid->kp * error + pid->ki * pid->integral[0] + pid->kd * derivative;
    
    // 限制输出
    *output = CLAMP(*output, -pid->output_limit, pid->output_limit);
    
    // 更新前一误差
    pid->prev_error[0] = error;
    
    return SUCCESS;
}

ErrorCode pid_reset(PID_Controller* pid) {
    if (!pid) return ERROR_INVALID_INPUT;
    
    for (int i = 0; i < 3; i++) {
        pid->integral[i] = 0.0;
        pid->prev_error[i] = 0.0;
    }
    
    return SUCCESS;
}

ErrorCode attitude_controller_set_target(AttitudeController* controller, Quaternion target) {
    if (!controller) return ERROR_INVALID_INPUT;
    
    controller->target_attitude = quaternion_normalize(target);
    controller->mode = ATTITUDE_MODE_POINTING;
    controller->is_active = true;
    
    return SUCCESS;
}

ErrorCode attitude_controller_update(AttitudeController* controller, double dt) {
    if (!controller || dt <= 0.0) return ERROR_INVALID_INPUT;
    
    if (!controller->is_active) return SUCCESS;
    
    // 计算姿态误差
    Quaternion error_quat = quaternion_multiply(
        quaternion_inverse(controller->current_attitude),
        controller->target_attitude
    );
    
    // 将四元数误差转换为轴角表示
    Vector3D error_axis = vector3d_create(error_quat.x, error_quat.y, error_quat.z);
    double error_angle = 2.0 * atan2(vector3d_magnitude(error_axis), error_quat.w);
    
    if (vector3d_magnitude(error_axis) > 1e-6) {
        error_axis = vector3d_normalize(error_axis);
    } else {
        error_axis = vector3d_create(1.0, 0.0, 0.0);
    }
    
    // 将误差分解到机体坐标系
    Vector3D body_error = quaternion_rotate_vector(
        controller->current_attitude,
        vector3d_scale(error_axis, error_angle)
    );
    
    // PID控制
    double torque_x, torque_y, torque_z;
    pid_update(&controller->pid_roll, body_error.x, dt, &torque_x);
    pid_update(&controller->pid_pitch, body_error.y, dt, &torque_y);
    pid_update(&controller->pid_yaw, body_error.z, dt, &torque_z);
    
    controller->control_torque[0] = torque_x;
    controller->control_torque[1] = torque_y;
    controller->control_torque[2] = torque_z;
    
    // 检查是否到达目标
    if (fabs(error_angle) < ATTITUDE_TOLERANCE) {
        controller->mode = ATTITUDE_MODE_STABILIZE;
        pid_reset(&controller->pid_roll);
        pid_reset(&controller->pid_pitch);
        pid_reset(&controller->pid_yaw);
    }
    
    return SUCCESS;
}

ErrorCode attitude_controller_get_torque(AttitudeController* controller, double torque[3]) {
    if (!controller || !torque) return ERROR_INVALID_INPUT;
    
    torque[0] = controller->control_torque[0];
    torque[1] = controller->control_torque[1];
    torque[2] = controller->control_torque[2];
    
    return SUCCESS;
}

ErrorCode attitude_controller_set_mode(AttitudeController* controller, AttitudeMode mode) {
    if (!controller) return ERROR_INVALID_INPUT;
    
    controller->mode = mode;
    
    if (mode == ATTITUDE_MODE_IDLE) {
        controller->is_active = false;
        pid_reset(&controller->pid_roll);
        pid_reset(&controller->pid_pitch);
        pid_reset(&controller->pid_yaw);
    } else {
        controller->is_active = true;
    }
    
    return SUCCESS;
}

Quaternion attitude_optimal_rotation(Quaternion from, Quaternion to) {
    from = quaternion_normalize(from);
    to = quaternion_normalize(to);
    
    return quaternion_multiply(quaternion_inverse(from), to);
}

Vector3D attitude_error_vector(Quaternion current, Quaternion target) {
    Quaternion error = attitude_optimal_rotation(current, target);
    
    Vector3D axis = vector3d_create(error.x, error.y, error.z);
    double angle = 2.0 * atan2(vector3d_magnitude(axis), error.w);
    
    if (vector3d_magnitude(axis) > 1e-6) {
        axis = vector3d_normalize(axis);
    } else {
        axis = vector3d_create(1.0, 0.0, 0.0);
    }
    
    return vector3d_scale(axis, angle);
}

double attitude_error_angle(Quaternion current, Quaternion target) {
    return quaternion_angle(current, target);
}

void attitude_print_status(const AttitudeController* controller) {
    if (!controller) return;
    
    printf("=== 姿态控制器状态 ===\n");
    printf("模式: %d\n", controller->mode);
    printf("激活: %s\n", controller->is_active ? "是" : "否");
    
    printf("当前姿态: ");
    quaternion_print("", controller->current_attitude);
    
    printf("目标姿态: ");
    quaternion_print("", controller->target_attitude);
    
    printf("控制力矩: [%.6f, %.6f, %.6f] N·m\n",
           controller->control_torque[0],
           controller->control_torque[1],
           controller->control_torque[2]);
    
    double error = attitude_error_angle(controller->current_attitude, controller->target_attitude);
    printf("姿态误差: %.6f 弧度 (%.2f 度)\n", error, error * RAD_TO_DEG);
}
